function [ qNorm ] = quatnormalize_pgs( q ) %#codegen
%QUATNORMALIZE_PGS 对四元数进行规范化，按P. G. Savage方法
%
% Tips:
% # 本函数不能1次完全规范化，迭代调用可减少残差
%
% Input Arguments:
% # q: 1*4向量，四元数
%
% Output Arguments:
% # qNorm: 1*4向量，规范化后的四元数
%
% References:
% # 《应用导航算法工程基础》“逐次规范化算法”

epsilonq = (quatmultiply(q, quatconj(q)) - [1 0 0 0]) / 2;
qNorm = quatmultiply([1 0 0 0]-epsilonq, q);
end